#include <Servo.h> //Roept de servo biblotheek aan om de servo motor aan te sturen

Servo myServo;  //Maakt het servo object aan 

// Ultrasoon sensor - Beschrijft aan welke pins wat verbonden is
const int trigPin = 6;
const int echoPin = 7;
const int buzzerPin = 4;

// Servo en potentiometer pins - Beschrijft aan welke pins wat verbonden is
int potPin = A0;   
int servoPin = 9;  
int val;           

// LED en button pins - Beschrijft aan welke pins wat verbonden is
const int buttonPin = 2;  
const int ledPin1 = 12;   
const int ledPin2 = 13;   
int buttonState = 0; //Maakt variable aan om te onthouden wat de knopstatus is
int lastButtonState = 0;     
int currentLed = 0;         

void setup() {
  // Servo setup - Koppelt de servo aan pin 9 en zet de juiste pin mode
  myServo.attach(servoPin);
  Serial.begin(9600); 

  // Ultrasonic en buzzer setup - Koppelt de ultra sonic en buzzer aan de pins en zet de juiste pin mode
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);

  // LEDs en button setup - Koppelt de LEDS en buttons aan de pins en zet de juiste pin mode
  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);
  pinMode(buttonPin, INPUT);

  digitalWrite(ledPin1, LOW);
  digitalWrite(ledPin2, LOW);
}

void loop() {
  // === Ultrasoon sensor met buzzer === Hiermee word een ultrasonic puls verstuurd
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH); //Hiermee wordt de tijd tot een echo gemeten de afstand is kleiner dan 3cm
  int distance = duration * 0.034 / 2;

  Serial.print("Afstand: ");
  Serial.print(distance);
  Serial.println(" cm"); //Zorgt ervoor dat er gemeten word in cm

  if (distance < 3) { //Afstand kleiner dan 3cm
    digitalWrite(buzzerPin, HIGH);
    delay(400);  //De buzzer piept voor 400ms
    digitalWrite(buzzerPin, LOW);
  }

  // === Servo aansturen via potentiometer === De waarde voor de potentiometer word uitgelezen
  val = analogRead(potPin);
  val = map(val, 0, 1023, 0, 180); //De gelezen waarde word omgezet naar een hoek van 0-180 graden
  myServo.write(val);

  Serial.print("Potentiometerwaarde: ");
  Serial.println(val);

  // === LED aansturen via knop ===
  buttonState = digitalRead(buttonPin); //Leest de status van de knop en dus of die wordt ingedrukt

  if (buttonState == HIGH && lastButtonState == LOW) { //Wanneer er niks word ingedrukt brand er geen lampje
    digitalWrite(ledPin1, LOW);
    digitalWrite(ledPin2, LOW);

    if (currentLed == 0) {
      digitalWrite(ledPin1, HIGH);
      delay(700);
      digitalWrite(ledPin1, LOW);
      currentLed = 1; //Wanneer knop wordt ingedrukt brand er een van de lampjes
    } else {
      digitalWrite(ledPin2, HIGH);
      delay(700);
      digitalWrite(ledPin2, LOW);
      currentLed = 0; //Wanneer de knop nig een keer word ingedrukt brand het ander lampje
    }
    delay(50);  
  }

  lastButtonState = buttonState;

  delay(200);  // Kleine algemene delay om alles netjes te laten verlopen
}